#include "inimu.h"

#include "math.h"
#include "stdlib.h"


#define HZ 100.0f

typedef struct{
    float ax_sum;
    float ay_sum;
    float az_sum;
    float ax_cal;
    float ay_cal;
    float az_cal;
    float counts;
    float ax_ret;
    float ay_ret;
    float az_ret;
    float ax_past;
    float ay_past;
    float az_past;
    float ax_now;
    float ay_now;
    float az_now;
    float running_samples_count;
		int running_samples_cout;
    float acc_x_save[100];
    float acc_y_save[100];
    float acc_z_save[100];
    float acc_x_save_sum;
    float acc_y_save_sum;
    float acc_z_save_sum;
    int couts_flag;
    float pos_x;
    float pos_y;
    float pos_x_past;
    float pos_y_past;
    float vel_x;
    float vel_y;
    float vel_x_past;
    float vel_y_past;
    float pos_x_der;
    float pos_y_der;
}Acc;

Acc acc={0.0f};

float dtr(float d)
{
    return (3.14159f*d)/180.0f;
}

float rtd(float r)
{
    return (r*180.0f)/(3.14159f);
}

void jiaozhun(float ax,float ay,float az)
{
    acc.counts++;
    acc.ax_sum+=ax;
    acc.ay_sum+=ay;
    acc.az_sum+=az;
    acc.ax_cal=acc.ax_sum/acc.counts;
    acc.ay_cal=acc.ay_sum/acc.counts;
    acc.az_cal=acc.az_sum/acc.counts;
}

void acc_lvbo(float ax,float ay,float az)
{
    acc.ax_now=ax;
    acc.ay_now=ay;
    acc.az_now=az;
    if(acc.running_samples_cout==20){
        acc.running_samples_cout=0;
        acc.acc_x_save_sum-=acc.acc_x_save[0];
        acc.acc_y_save_sum-=acc.acc_y_save[0];
        acc.acc_z_save_sum-=acc.acc_z_save[0];
        acc.couts_flag=1;
				
    }
    if(acc.running_samples_cout<20) {
        acc.acc_x_save[acc.running_samples_cout]=acc.ax_now;
        acc.acc_y_save[acc.running_samples_cout]=acc.ay_now;
        acc.acc_z_save[acc.running_samples_cout]=acc.az_now;
        acc.acc_x_save_sum+=acc.ax_now;
        acc.acc_y_save_sum+=acc.ay_now;
        acc.acc_z_save_sum+=acc.az_now;
        acc.running_samples_cout++;
				acc.running_samples_count++;
    }
    if(acc.couts_flag==1){
        acc.ax_ret=acc.acc_x_save_sum/20.0f-acc.ax_cal;
        acc.ay_ret=acc.acc_y_save_sum/20.0f-acc.ay_cal;
        acc.az_ret=acc.acc_z_save_sum/20.0f-acc.az_cal;
    }
    else{
        acc.ax_ret=acc.acc_x_save_sum/ acc.running_samples_count-acc.ax_cal;
        acc.ay_ret=acc.acc_y_save_sum/ acc.running_samples_count-acc.ay_cal;
        acc.az_ret=acc.acc_z_save_sum/ acc.running_samples_count-acc.az_cal;
    }
    acc.ax_past=acc.ax_ret;
    acc.ay_past=acc.ay_ret;
    acc.az_past=acc.az_ret;

}

void Inertial_navigation(float ax,float ay,float az,int dir,int dir_change,int move_state)//
{
    float deg45=sin(dtr(45));
    if(move_state==0){//stop
        jiaozhun(ax,ay,az);
    }
    else if(move_state==1){
        acc_lvbo(ax,ay,az);
        if(dir_change==1){
            return;
        }
        else{
            acc.vel_x=acc.vel_x_past+acc.ax_past+((acc.ax_ret-acc.ax_past)/2.0f);
            acc.vel_y=acc.vel_y_past+acc.ay_past+((acc.ay_ret-acc.ay_past)/2.0f);
            acc.pos_x_der=acc.vel_x_past+((acc.vel_x-acc.vel_x_past)/2.0f);
            acc.pos_y_der=acc.vel_y_past+((acc.vel_y-acc.vel_y_past)/2.0f);
            switch (dir) {
                case 1:
                    acc.pos_x=acc.pos_x_past+acc.pos_x_der;
                    acc.pos_y=acc.pos_y_past+acc.pos_y_der;
                    break;
                case 2:
                    acc.pos_x=acc.pos_x_past+acc.pos_x_der*deg45+acc.pos_y_der*deg45;
                    acc.pos_y=acc.pos_y_past-acc.pos_x_der*deg45+acc.pos_y_der*deg45;
                    break;
                case 3:
                    acc.pos_x=acc.pos_x_past+acc.pos_y_der;
                    acc.pos_y=acc.pos_y_past-acc.pos_x_der;
                    break;
                case 4:
                    acc.pos_x=acc.pos_x_past-acc.pos_x_der*deg45+acc.pos_y_der*deg45;
                    acc.pos_y=acc.pos_y_past-acc.pos_x_der*deg45-acc.pos_y_der*deg45;
                    break;
                case 5:
                    acc.pos_x=acc.pos_x_past-acc.pos_x_der;
                    acc.pos_y=acc.pos_y_past-acc.pos_y_der;
                    break;
                case 6:
                    acc.pos_x=acc.pos_x_past-acc.pos_x_der*deg45-acc.pos_y_der*deg45;
                    acc.pos_y=acc.pos_y_past+acc.pos_x_der*deg45-acc.pos_y_der*deg45;
                    break;
                case 7:
                    acc.pos_x=acc.pos_x_past-acc.pos_y_der;
                    acc.pos_y=acc.pos_y_past+acc.pos_x_der;
                    break;
                case 8:
                    acc.pos_x=acc.pos_x_past+acc.pos_x_der*deg45-acc.pos_y_der*deg45;
                    acc.pos_y=acc.pos_y_past+acc.pos_x_der*deg45+acc.pos_y_der*deg45;
                    break;
            }
            acc.vel_x_past=acc.vel_x;
            acc.vel_y_past=acc.vel_y;
            acc.pos_x_past=acc.pos_x;
            acc.pos_y_past=acc.pos_y;
        }
    }

}

//int main()
//{
//    float ax,ay,az;
//    srand((unsigned)time(NULL));
//    for(int i=0;i<500;i++){
//        ax=(rand()%100)/100.0f*pow(-1,rand());
//        ay=(rand()%100)/100.0f*pow(-1,rand());
//        az=(rand()%100)/100.0f*pow(-1,rand());
//        Inertial_navigation(ax,ay,az,1,0,0);
//    }
//    for(int i=0;i<500;i++){
//        ax=(rand()%100)/100.0f*pow(-1,rand());
//        ay=(rand()%100)/100.0f*pow(-1,rand());
//        az=(rand()%100)/100.0f*pow(-1,rand());
//        Inertial_navigation(ax,ay,az,1,0,1);
//        printf("%.3f  %.3f  %.3f\n%.3f  %.3f\n%.3f  %.3f\n%.3f  %.3f\n\n",acc.ax_ret,acc.ay_ret,acc.az_ret,acc.vel_x,acc.vel_y,acc.pos_x_der,acc.pos_y_der,acc.pos_x,acc.pos_y);
//    }

//}



